/* USER CODE BEGIN Header */
/**
 ******************************************************************************
 * File Name          : freertos.c
 * Description        : Code for freertos applications
 ******************************************************************************
 * @attention
 *
 * Copyright (c) 2024 STMicroelectronics.
 * All rights reserved.
 *
 * This software is licensed under terms that can be found in the LICENSE file
 * in the root directory of this software component.
 * If no LICENSE file comes with this software, it is provided AS-IS.
 *
 ******************************************************************************
 */
/* USER CODE END Header */

/* Includes ------------------------------------------------------------------*/
#include "FreeRTOS.h"
#include "task.h"
#include "main.h"
#include "cmsis_os.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include <string.h>
#include "ultrasonic.h"
#include "oled.h"
#include "gui.h"
#include "can.h"
/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */

/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */
/* Test Can */
#define CAN_TEST_MOD (0)
// 滤波器编号
#define CAN_FILTER(x) ((x) << 3)

// 接收队列
#define CAN_FIFO_0 (0 << 2)
#define CAN_FIFO_1 (1 << 2)

// 标准帧或扩展帧
#define CAN_STDID (0 << 1)
#define CAN_EXTID (1 << 1)

// 数据帧或遥控帧
#define CAN_DATA_TYPE (0 << 0)
#define CAN_REMOTE_TYPE (1 << 0)
/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN Variables */
uint16_t UltrasonicWarnDis = 30;
uint8_t BzPercent = 100;

CAN_TxHeaderTypeDef Can_TxHandleMsg;
CAN_RxHeaderTypeDef Can_RxHandleMsg;

uint8_t RxCanBuff[30] = {0};
__IO uint8_t Can_Rx_Flag = 0;

/* USER CODE END Variables */
osThreadId BuzzerTaskHandle;
osThreadId DisplayTaskHandle;
osThreadId UltrasonicTaskHandle;
osThreadId TransmitTaskHandle;
osThreadId KeyTaskHandle;

/* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN FunctionPrototypes */
void Can_FilterConfigure(CAN_HandleTypeDef *hcanhandle, uint16_t Object_Para, uint32_t ID, uint32_t Mask_ID);
void Can_Send(CAN_HandleTypeDef *hcanhandle, uint16_t ID, uint8_t *Data, uint16_t Length);

/* USER CODE END FunctionPrototypes */

void BuzzerTask500ms(void const *argument);
void DisplayTask100ms(void const *argument);
void UltrasonicTask50ms(void const *argument);
void TransmitTask100ms(void const *argument);
void KeyTask50ms(void const *argument);

void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */

/* GetIdleTaskMemory prototype (linked to static allocation support) */
void vApplicationGetIdleTaskMemory(StaticTask_t **ppxIdleTaskTCBBuffer, StackType_t **ppxIdleTaskStackBuffer, uint32_t *pulIdleTaskStackSize);

/* USER CODE BEGIN GET_IDLE_TASK_MEMORY */
static StaticTask_t xIdleTaskTCBBuffer;
static StackType_t xIdleStack[configMINIMAL_STACK_SIZE];

void vApplicationGetIdleTaskMemory(StaticTask_t **ppxIdleTaskTCBBuffer, StackType_t **ppxIdleTaskStackBuffer, uint32_t *pulIdleTaskStackSize)
{
    *ppxIdleTaskTCBBuffer = &xIdleTaskTCBBuffer;
    *ppxIdleTaskStackBuffer = &xIdleStack[0];
    *pulIdleTaskStackSize = configMINIMAL_STACK_SIZE;
    /* place for user code */
}
/* USER CODE END GET_IDLE_TASK_MEMORY */

/**
 * @brief  FreeRTOS initialization
 * @param  None
 * @retval None
 */
void MX_FREERTOS_Init(void)
{
    /* USER CODE BEGIN Init */

    /* USER CODE END Init */

    /* USER CODE BEGIN RTOS_MUTEX */
    /* add mutexes, ... */
    /* USER CODE END RTOS_MUTEX */

    /* USER CODE BEGIN RTOS_SEMAPHORES */
    /* add semaphores, ... */
    /* USER CODE END RTOS_SEMAPHORES */

    /* USER CODE BEGIN RTOS_TIMERS */
    /* start timers, add new ones, ... */
    /* USER CODE END RTOS_TIMERS */

    /* USER CODE BEGIN RTOS_QUEUES */
    /* add queues, ... */
    /* USER CODE END RTOS_QUEUES */

    /* Create the thread(s) */
    /* definition and creation of BuzzerTask */
    osThreadDef(BuzzerTask, BuzzerTask500ms, osPriorityNormal, 0, 128);
    BuzzerTaskHandle = osThreadCreate(osThread(BuzzerTask), NULL);

    /* definition and creation of DisplayTask */
    osThreadDef(DisplayTask, DisplayTask100ms, osPriorityNormal, 0, 128);
    DisplayTaskHandle = osThreadCreate(osThread(DisplayTask), NULL);

    /* definition and creation of UltrasonicTask */
    osThreadDef(UltrasonicTask, UltrasonicTask50ms, osPriorityHigh, 0, 128);
    UltrasonicTaskHandle = osThreadCreate(osThread(UltrasonicTask), NULL);

    /* definition and creation of TransmitTask */
    osThreadDef(TransmitTask, TransmitTask100ms, osPriorityNormal, 0, 128);
    TransmitTaskHandle = osThreadCreate(osThread(TransmitTask), NULL);

    /* definition and creation of KeyTask */
    osThreadDef(KeyTask, KeyTask50ms, osPriorityNormal, 0, 128);
    KeyTaskHandle = osThreadCreate(osThread(KeyTask), NULL);

    /* USER CODE BEGIN RTOS_THREADS */
    /* add threads, ... */
    /* USER CODE END RTOS_THREADS */
}

/* USER CODE BEGIN Header_BuzzerTask500ms */
/**
 * @brief  Function implementing the BuzzerTask thread.
 * @param  argument: Not used
 * @retval None
 */
/* USER CODE END Header_BuzzerTask500ms */
void BuzzerTask500ms(void const *argument)
{
    /* USER CODE BEGIN BuzzerTask500ms */
    BzPercent = 100;
    /* Infinite loop */
    for (;;)
    {
        if (BzPercent >= 100)
        {
            HAL_GPIO_WritePin(BUZZER_GPIO_Port, BUZZER_Pin, GPIO_PIN_RESET);
            HAL_GPIO_TogglePin(LED1_GPIO_Port, LED1_Pin);
            osDelay(500);
        }
        else
        {
            HAL_GPIO_TogglePin(BUZZER_GPIO_Port, BUZZER_Pin);
            HAL_GPIO_TogglePin(LED1_GPIO_Port, LED1_Pin);
            osDelay(20 + (BzPercent * 2));
        }
    }
    /* USER CODE END BuzzerTask500ms */
}

/* USER CODE BEGIN Header_DisplayTask100ms */
/**
 * @brief Function implementing the DisplayTask thread.
 * @param argument: Not used
 * @retval None
 */
/* USER CODE END Header_DisplayTask100ms */
void DisplayTask100ms(void const *argument)
{
    /* USER CODE BEGIN DisplayTask100ms */
    char DisplayStr[20] = {0};
    uint8_t StrSize = sizeof(DisplayStr);

    taskENTER_CRITICAL();
    OLED_Init();
    taskEXIT_CRITICAL();
    /* Draw line. */
    GUI_DrawLine(0, 0, 127, 0, 1);
    GUI_DrawLine(0, 63, 127, 63, 1);
    GUI_DrawLine(0, 0, 0, 63, 1);
    GUI_DrawLine(127, 0, 127, 63, 1);
    GUI_DrawLine(85, 0, 85, 128, 1);
    /* Show string. */
    GUI_ShowString(2, 1, "U1: ", 16, 1);
    GUI_ShowString(2, 16, "U2: ", 16, 1);
    GUI_ShowString(2, 32, "U3: ", 16, 1);
    GUI_ShowString(2, 47, "U4: ", 16, 1);
    GUI_ShowString(88, 16, "WARN", 16, 1);

    /* Infinite loop */
    for (;;)
    {
        /* Display ulransonic data. */
        memset(DisplayStr, 0, StrSize);
        sprintf(DisplayStr, "%.2f", Ultrasonic_GetDisCm(0));
        GUI_ShowString(32, 1, (uint8_t *)DisplayStr, 16, 1);
        memset(DisplayStr, 0, StrSize);
        sprintf(DisplayStr, "%.2f", Ultrasonic_GetDisCm(1));
        GUI_ShowString(32, 16, (uint8_t *)DisplayStr, 16, 1);
        memset(DisplayStr, 0, StrSize);
        sprintf(DisplayStr, "%.2f", Ultrasonic_GetDisCm(2));
        GUI_ShowString(32, 32, (uint8_t *)DisplayStr, 16, 1);
        memset(DisplayStr, 0, StrSize);
        sprintf(DisplayStr, "%.2f", Ultrasonic_GetDisCm(3));
        GUI_ShowString(32, 47, (uint8_t *)DisplayStr, 16, 1);
        /* Display warning distence. */
        memset(DisplayStr, 0, StrSize);
        sprintf(DisplayStr, "%3d", UltrasonicWarnDis);
        GUI_ShowString(90, 32, (uint8_t *)DisplayStr, 16, 1);
        /* Refresh display. */
        taskENTER_CRITICAL();
        OLED_Display();
        taskEXIT_CRITICAL();

        osDelay(100);
    }
    /* USER CODE END DisplayTask100ms */
}

/* USER CODE BEGIN Header_UltrasonicTask50ms */
/**
 * @brief Function implementing the UltrasonicTask thread.
 * @param argument: Not used
 * @retval None
 */
/* USER CODE END Header_UltrasonicTask50ms */
void UltrasonicTask50ms(void const *argument)
{
    /* USER CODE BEGIN UltrasonicTask50ms */
    double MinVtsValue = 0;
    uint8_t i = 1;

    Ultrasonic_Init();
    /* Infinite loop */
    for (;;)
    {
        Ultrasonic_Task();

        MinVtsValue = UltrasonicWarnDis;
        for (i = 0; i < UltrasonicQuantity; i++)
        {
            MinVtsValue = (MinVtsValue > Ultrasonic_GetDisCm(i)) ? (Ultrasonic_GetDisCm(i)) : (MinVtsValue);
        }
        if (MinVtsValue < UltrasonicWarnDis)
        {
            BzPercent = (uint8_t)((MinVtsValue / UltrasonicWarnDis) * 100);
        }
        else
        {
            BzPercent = 100;
        }

        osDelay(50);
    }
    /* USER CODE END UltrasonicTask50ms */
}

/* USER CODE BEGIN Header_TransmitTask100ms */
/**
 * @brief Function implementing the TransmitTask thread.
 * @param argument: Not used
 * @retval None
 */
/* USER CODE END Header_TransmitTask100ms */
void TransmitTask100ms(void const *argument)
{
    /* USER CODE BEGIN TransmitTask100ms */
    uint8_t i = 0;
    uint8_t TxCanBuff[8] = {0xFF}; /* Can buffer */
    uint16_t TargetId = 0x1234;    /* Target ID */
    uint16_t IntData = 0;          /* integer data */
    uint8_t FloatData = 0;         /* small data */

    Can_FilterConfigure(&hcan, CAN_FILTER(1) | CAN_FIFO_0 | CAN_STDID | CAN_DATA_TYPE, 0x1111, 0x00);
    HAL_CAN_Start(&hcan);
    HAL_CAN_ActivateNotification(&hcan, CAN_IT_RX_FIFO0_MSG_PENDING);

    printf("init. \r\n");

    /* Infinite loop */
    for (;;)
    {
        /* Can bus */
        for (i = 0; i < UltrasonicQuantity; i++)
        {
            memset(TxCanBuff, 0xFF, 8);
            TxCanBuff[0] = i;                                                     /* First Byte Ultrasonic ID. */
            TxCanBuff[1] = (Ultrasonic_GetState(i) == UTS_RESULT_OK) ? (1) : (0); /* Second Byte Ultrasonic Status. 1: ok,  other is not good. */
            if (Ultrasonic_GetState(i) == UTS_RESULT_OK)
            {
                IntData = (uint16_t)Ultrasonic_GetDisCm(i);
                FloatData = (uint8_t)(Ultrasonic_GetDisCm(i) * 100) % 100;
                /* 2-byte integer data. */
                TxCanBuff[2] = (uint8_t)((IntData >> 8) & 0xFF); /* High-Byte */
                TxCanBuff[3] = (uint8_t)((IntData) & 0xFF);      /* Low-Byte */
                /* 1 byte of small data */
                TxCanBuff[4] = FloatData;
            }
            Can_Send(&hcan, TargetId, TxCanBuff, 8);
#if CAN_TEST_MOD
            osDelay(50);
#else
            osDelay(5);
#endif
        }

        // printf("D1: %.2f, D12: %.2f, D3: %.2f, D4: %.2f, us: %d  %d  %d  %d \r\n", Ultrasonic_GetDisCm(0), Ultrasonic_GetDisCm(1), Ultrasonic_GetDisCm(2), Ultrasonic_GetDisCm(3),
        //        UltrasonicData[0].State, UltrasonicData[1].State, UltrasonicData[2].State, UltrasonicData[3].State);
        osDelay(100);
    }
    /* USER CODE END TransmitTask100ms */
}

/* USER CODE BEGIN Header_KeyTask50ms */
/**
 * @brief Function implementing the KeyTask thread.
 * @param argument: Not used
 * @retval None
 */
/* USER CODE END Header_KeyTask50ms */
void KeyTask50ms(void const *argument)
{
    /* USER CODE BEGIN KeyTask50ms */
    uint8_t Key1Cnt = 0, Key2Cnt = 0;

    /* Infinite loop */
    for (;;)
    {
#if CAN_TEST_MOD
        if (Can_Rx_Flag)
        {
            Can_Rx_Flag = 0;

            double TempData = (RxCanBuff[2] << 8 | RxCanBuff[3]) + ((float)RxCanBuff[4] / 100);
            printf("ID: %d, State: %d, Data: %.2f \r\n", RxCanBuff[0], RxCanBuff[1], TempData);
            if (RxCanBuff[1] == 1)
            {
                double TempData = (RxCanBuff[2] << 8 | RxCanBuff[3]) + ((float)RxCanBuff[4] / 100);
                printf("ID: %d, State: %d, Data: %.2f \r\n", RxCanBuff[0], RxCanBuff[1], TempData);
            }
            else
            {
                printf("ID: %d, State: Error. \r\n", RxCanBuff[0]);
            }
        }

        if (HAL_GPIO_ReadPin(KEY1_GPIO_Port, KEY1_Pin) == GPIO_PIN_RESET)
        {
            if (Key1Cnt++ >= 10)
            {
                Key1Cnt = 10;
                UltrasonicWarnDis = (UltrasonicWarnDis >= 200) ? (200) : (UltrasonicWarnDis + 1);
            }
        }
        else
        {
            Key1Cnt = 0;
        }
        if (HAL_GPIO_ReadPin(KEY2_GPIO_Port, KEY2_Pin) == GPIO_PIN_RESET)
        {
            if (Key2Cnt++ >= 10)
            {
                Key2Cnt = 10;
                UltrasonicWarnDis = (UltrasonicWarnDis == 0) ? (0) : (UltrasonicWarnDis - 1);
            }
        }
        else
        {
            Key2Cnt = 0;
        }
        osDelay(10);
#else
        if (HAL_GPIO_ReadPin(KEY1_GPIO_Port, KEY1_Pin) == GPIO_PIN_RESET)
        {
            if (Key1Cnt++ >= 2)
            {
                Key1Cnt = 3;
                UltrasonicWarnDis = (UltrasonicWarnDis >= 200) ? (200) : (UltrasonicWarnDis + 1);
            }
        }
        else
        {
            Key1Cnt = 0;
        }
        if (HAL_GPIO_ReadPin(KEY2_GPIO_Port, KEY2_Pin) == GPIO_PIN_RESET)
        {
            if (Key2Cnt++ >= 2)
            {
                Key2Cnt = 3;
                UltrasonicWarnDis = (UltrasonicWarnDis == 0) ? (0) : (UltrasonicWarnDis - 1);
            }
        }
        else
        {
            Key2Cnt = 0;
        }
        osDelay(50);
#endif
    }
    /* USER CODE END KeyTask50ms */
}

/* Private application code --------------------------------------------------*/
/* USER CODE BEGIN Application */
void Can_FilterConfigure(CAN_HandleTypeDef *hcanhandle, uint16_t Object_Para, uint32_t ID, uint32_t Mask_ID)
{
    CAN_FilterTypeDef Can_FilterInit;

    // 检测关键传参
    assert_param(hcanhandle != NULL);

    if ((Object_Para & 0x02))
    {
        // 数据帧
        // 掩码后ID的高16bit
        Can_FilterInit.FilterIdHigh = ID << 3 << 16;
        // 掩码后ID的低16bit
        Can_FilterInit.FilterIdLow = ID << 3 | ((Object_Para & 0x03) << 1);
        // ID掩码值高16bit
        Can_FilterInit.FilterMaskIdHigh = Mask_ID << 3 << 16;
        // ID掩码值低16bit
        Can_FilterInit.FilterMaskIdLow = Mask_ID << 3 | ((Object_Para & 0x03) << 1);
    }
    else
    {
        // 遥控帧
        // 掩码后ID的高16bit
        Can_FilterInit.FilterIdHigh = ID << 5;
        // 掩码后ID的低16bit
        Can_FilterInit.FilterIdLow = ((Object_Para & 0x03) << 1);
        // ID掩码值高16bit
        Can_FilterInit.FilterMaskIdHigh = Mask_ID << 5;
        // ID掩码值低16bit
        Can_FilterInit.FilterMaskIdLow = ((Object_Para & 0x03) << 1);
    }

    // 滤波器序号, 0-27, 共28个滤波器, can1是0~13, can2是14~27
    Can_FilterInit.FilterBank = Object_Para >> 3;
    // 滤波器绑定FIFOx, 只能绑定一个
    Can_FilterInit.FilterFIFOAssignment = (Object_Para >> 2) & 0x01;
    // 使能滤波器
    Can_FilterInit.FilterActivation = ENABLE;
    // 滤波器模式, 设置ID掩码模式
    Can_FilterInit.FilterMode = CAN_FILTERMODE_IDMASK;
    // 32位滤波
    Can_FilterInit.FilterScale = CAN_FILTERSCALE_32BIT;
    // 从机模式选择开始单元
    Can_FilterInit.SlaveStartFilterBank = 14;

    if (HAL_CAN_ConfigFilter(hcanhandle, &Can_FilterInit) != HAL_OK)
    {
        Error_Handler();
    }
}

void Can_Send(CAN_HandleTypeDef *hcanhandle, uint16_t ID, uint8_t *Data, uint16_t Length)
{
    uint32_t pTxMailbox;

    Can_TxHandleMsg.StdId = ID;
    Can_TxHandleMsg.ExtId = 0x00;
    Can_TxHandleMsg.IDE = 0x00;
    Can_TxHandleMsg.RTR = 0x00;
    Can_TxHandleMsg.DLC = Length;

    HAL_CAN_AddTxMessage(hcanhandle, &Can_TxHandleMsg, Data, &pTxMailbox);
}

void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
{
    if (hcan->Instance == CAN1)
    {
        HAL_CAN_GetRxMessage(hcan, CAN_FILTER_FIFO0, &Can_RxHandleMsg, RxCanBuff);
        Can_Rx_Flag = 1;
    }
}
/* USER CODE END Application */
